/*
 * StringUtils.cpp
 *
 *  Created on: Feb 28, 2012
 *
 *  Created on: Dec 12, 2011
 *      Author: Michael Anderson
 *      Part of Team 955 Battery Cart project
 *      compiled on Eclipse with Arduino libraries and avr-gcc
 */

#include "Utils.h"

Utils::Utils() { }

char Utils::getOnesDigit(uint16_t input) {
    return (input % 10) + '0';
}

void Utils::doubleToCharArray(char* buf, uint8_t length, double d, uint8_t precision) {
    bool negative = false;
    if(d < 0) {
        negative = true;
        d *= -1;
    }
    uint16_t dAsInt = (uint16_t)d;
    uint8_t magnitude = 1;

    while (true) {
        if (dAsInt >= 10) {
            magnitude++;
            dAsInt /= 10;
        }
        else
            break;
    }
    uint8_t buffer[magnitude + precision + 2 + negative]; //leave space for decimal and null
    if(negative)
        buffer[0] = '-';

    dAsInt = (uint16_t)d;

    for (int16_t i = magnitude - 1; i >= 0; i--) {
        buffer[i + negative] = getOnesDigit(dAsInt);
        dAsInt /= 10;
    }
    buffer[magnitude + negative] = '.';

    uint8_t j;
    for (j = 0; j < precision; j++) {
        buffer[j + magnitude + 1 + negative] = getOnesDigit((uint16_t)(d * ((double)(10 ^ j))));
    }
    buffer[j + magnitude + 1 + negative] = '\0';

    uint8_t k;
    for (k = 0; k < length; k++) {
        if (buffer[k] == '\0') {
            buf[k] = buffer[k];
            return;
        }
        else if (buffer[k] == '.') {
            // If there is room for one or more decimal places, include them.
            // Otherwise, leave them out.
            if (k + 1 < length) {
                buf[k] = buffer[k];
            }
            else {
                buf[k] = '\0';
                return;
            }
        }
        else {
            buf[k] = buffer[k];
        }
    }
    buf[k++] = '\0';
}

uint16_t Utils::safeAnalogRead(uint8_t in) {
    delay(ANALOG_READ_DELAY);
    analogRead(in);
    delay(ANALOG_READ_DELAY);
    return analogRead(in);
}

uint16_t Utils::filteredAnalogRead(uint8_t pin, analog_read_filter_type filter) {
    uint_fast16_t samples[(int)ANALOG_READ_SAMPLE_LEN];
    analogRead(pin);
    delay(ANALOG_READ_DELAY);
    for (uint_fast16_t i = 0; i < (int)ANALOG_READ_SAMPLE_LEN; i++) {
        samples[i] = analogRead(pin);
    }

    switch(filter) {
        case ANALOG_READ_FILTER_HIGHEST: {
            uint16_t highest = 0;
            uint_fast16_t sampleLen = (uint_fast16_t)ANALOG_READ_SAMPLE_LEN;
            for (uint_fast16_t i = 0; i < sampleLen; i++) {
                if (samples[i] > highest)
                    highest = samples[i];
            }
            return highest;
            break;
        }
        case ANALOG_READ_FILTER_LOWEST: {
            uint16_t lowest = samples[0];
            uint_fast16_t sampleLen = (uint_fast16_t)ANALOG_READ_SAMPLE_LEN;
            for (uint_fast16_t i = 1; i < sampleLen; i++) {
                if (samples[i] < lowest)
                    lowest = samples[i];
            }
            return lowest;
            break;
        }
        case ANALOG_READ_FILTER_MEAN: {
            uint_fast32_t tmp = 0;
            uint_fast16_t sampleLen = (uint_fast16_t)ANALOG_READ_SAMPLE_LEN;
            for (uint_fast16_t i = 0; i < sampleLen; i++) {
                tmp += samples[i];
            }
            return tmp / ANALOG_READ_SAMPLE_LEN;
            break;
        }
        case ANALOG_READ_FILTER_RMS: {
            uint_fast32_t tmp = 0;
            uint_fast16_t sampleLen = (uint_fast16_t)ANALOG_READ_SAMPLE_LEN;
            for (uint_fast16_t i = 0; i < sampleLen; i++) {
                tmp += ((uint32_t)samples[i]) * ((uint32_t)samples[i]);
            }
            tmp /= ANALOG_READ_SAMPLE_LEN;
            return sqrt(tmp);
            break;
        }
    }
    return 0;
}

/*void Utils::showError(uint8_t* message) {
    if (displaysInitialized) {
        uint8_t buffer[DISPLAY_COLS + 1];
        uint8_t i = 0;
        while (true) {
            if (i == DISPLAY_COLS || message[i] == '\0') {
                buffer[i] = '\0';
                break;
            }
            else {
                buffer[i] = message[i];
            }
            i++;
        }
        displays[DISPLAY_PRIMARY]->setFullScreen(true);
        displays[DISPLAY_PRIMARY]->clear(false);
        displays[DISPLAY_PRIMARY]->setLine(0, "FATAL:");
        displays[DISPLAY_PRIMARY]->setLine(1, buffer);
        displays[DISPLAY_PRIMARY]->lock();
    }
}*/
